#include "fonctions.h"

int write_in_queue(RT_QUEUE *msgQueue, void * data, int size);

/** 
 * Envoie un message du superviseur au moniteur.
 */
void envoyer(void * arg) {
    DMessage *msg;
    int err, status, status_send = 0;
    
    while (1) {
        rt_printf("tenvoyer : Attente d'un message\n");  
                
        if ((err = rt_queue_read(&queueMsgGUI, &msg, sizeof (DMessage), TM_INFINITE)) >= 0) {
            rt_printf("tenvoyer : envoi d'un message au moniteur\n");
            
            rt_mutex_acquire(&mutexEtat, TM_INFINITE);
            status = etatCommMoniteur;
            rt_mutex_release(&mutexEtat);    
            
            if (status == STATUS_OK){
                rt_mutex_acquire(&mutexServeur, TM_INFINITE);
                status_send = serveur->send(serveur, msg);
                rt_mutex_release(&mutexServeur);
                
                if (status_send <= 0){
                    perte_com_moniteur(-1);
                }
            }
            msg->free(msg);
        } else {
            rt_printf("Error msg queue write: %s\n", strerror(-err));
        }
               
    }
}


/** 
 * Etablie la connexion avec le robot 
 * Actif lorsqu'on peut prendre le sémaphore semConnecterRobot
 * Il faut choisir la valeur de "mode" pour sélectionner le mode de connexion 
 *  
 * PRIORITE : 20
 */
void connecter(void * arg) {
    int status;
    DMessage *message;    
    int compteur, bool_envoye ; 
    
                    /******** IMPORTANT ********/
    /* Le mode permet de choisir si on lance le robot avec le watchdog ou non.
     * Mode = 1 : SANS Watchdog
     * Mode = 2 : AVEC Watchdog
     */
    int mode = 2 ;
    
    rt_printf("tconnect : Debut de l'exécution de tconnect\n");
    
    while (1) {
        rt_printf("tconnect : Attente du sémarphore semConnecterRobot\n");
        rt_sem_p(&semConnecterRobot, TM_INFINITE);
        rt_printf("tconnect : Ouverture de la communication avec le robot\n");
        status = robot->open_device(robot);
        
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommRobot = status;
        rt_mutex_release(&mutexEtat);
        
        if (status == STATUS_OK) {
            
            /* Tentatives succéssives d'envoyer un message au robot
             * Fonctions : 
             *   start_securely()
             *   start() 
             */
            bool_envoye = 0 ; 
            compteur = 0 ;
            while (compteur < MAX_TEST_COM && !bool_envoye){
                /* Le choix du type de connexion est donc effictif ici. */
                switch (mode){
                    case 1:
                        status = robot->start_insecurely(robot);
                        break ;
                    case 2 :
                        status = d_robot_start(robot);                    
                        
                        if (status == STATUS_OK){
                            rt_sem_v(&semWatchDog) ;
                        }
                        break ;
                }
                rt_printf("tconnecter: status après robot start : %d\n",status);
                if (status != STATUS_OK){
                    rt_mutex_acquire(&mutexCompteurCommRobot, TM_INFINITE) ;
                    compteurEchecCommRobot++ ;
                    compteur = compteurEchecCommRobot ;
                    rt_mutex_release(&mutexCompteurCommRobot) ;
                }else{
                    rt_mutex_acquire(&mutexCompteurCommRobot, TM_INFINITE) ;
                    compteurEchecCommRobot = 0 ;
                    compteur = compteurEchecCommRobot ;
                    bool_envoye = 1 ;
                    rt_mutex_release(&mutexCompteurCommRobot) ;
                }
            }   
            
            if (status == STATUS_OK){
                rt_printf("tconnect : Robot démarrer\n");                
                rt_sem_v(&semBatterie);                
            }
            
            rt_mutex_acquire(&mutexEtat, TM_INFINITE);
            etatCommRobot = status;
            rt_mutex_release(&mutexEtat);
        }        
        
        
        message = d_new_message();
        message->put_state(message, status);
        
        rt_printf("tconnecter : Envoi message\n");
        message->print(message, 100);
        
        if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
            message->free(message);
        }
    }
}

/** 
 * Reçoit et répartie les messages reçus par le superviseur  
 */
void communiquer(void *arg) {
    DMessage *msg = d_new_message();
    int status ;
    int num_msg = 0;
    int status_server ;
    
    while (1) {
        rt_printf("tserver : Début de l'exécution de serveur\n");
        
        serveur->open(serveur, "8000");
        status = STATUS_OK ; 
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommMoniteur = status;
        rt_mutex_release(&mutexEtat);
        
        rt_printf("tserver : Connexion\n");       

        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommMoniteur = status;
        rt_mutex_release(&mutexEtat);
        while(status == STATUS_OK){
            rt_printf("tserver : Attente d'un message\n");
            
            status_server = serveur->receive(serveur, msg);
            
            num_msg++;
            if (status_server > 0) {
                switch (msg->get_type(msg)) {
                    case MESSAGE_TYPE_ACTION:
                        rt_printf("tserver : Le message %d reçu est une action\n",
                                num_msg);
                        DAction *action = d_new_action();
                        action->from_message(action, msg);
                        switch (action->get_order(action)) {
                            case ACTION_CONNECT_ROBOT:
                                rt_printf("tserver : Action connecter robot\n");
                                rt_sem_v(&semConnecterRobot);
                                break;
                            case ACTION_FIND_ARENA:
                                rt_printf("tserver : Action Find Arena\n") ;
                                rt_mutex_acquire(&mutexActionImage, TM_INFINITE) ;
                                actionImage = ACTION_FIND_ARENA ;
                                rt_mutex_release(&mutexActionImage) ;
                                rt_sem_v(&semCalibrer);
                                break ;
                            case ACTION_ARENA_IS_FOUND:
                                rt_printf("tserver : Action Area Is Found\n") ;
                                rt_mutex_acquire(&mutexActionImage, TM_INFINITE) ;
                                actionImage = ACTION_ARENA_IS_FOUND ;
                                rt_mutex_release(&mutexActionImage) ; 
                                rt_sem_v(&semCalibrer);
                                break ;
                            case ACTION_ARENA_FAILED:
                                rt_printf("tserver : Action Area Failed\n") ;
                                rt_mutex_acquire(&mutexActionImage, TM_INFINITE) ;
                                actionImage = ACTION_ARENA_FAILED ;
                                rt_mutex_release(&mutexActionImage); 
                                rt_sem_v(&semCalibrer);
                                break ;
                            case ACTION_COMPUTE_CONTINUOUSLY_POSITION:
                                rt_printf("tserver : Action Compute Continously Position\n") ;
                                rt_mutex_acquire(&mutexActionImage, TM_INFINITE) ;
                                actionImage = ACTION_COMPUTE_CONTINUOUSLY_POSITION ;
                                rt_mutex_release(&mutexActionImage) ;
                                break ;
                            case ACTION_STOP_COMPUTE_POSITION:
                                rt_printf("tserver : Action Stop Compute position\n") ;
                                rt_mutex_acquire(&mutexActionImage, TM_INFINITE) ;
                                actionImage = ACTION_STOP_COMPUTE_POSITION ;
                                rt_mutex_release(&mutexActionImage) ;
                                break;
                        }
                        break;
                    case MESSAGE_TYPE_MOVEMENT:
                        rt_printf("tserver : Le message reçu %d est un mouvement\n",
                                num_msg);
                        rt_mutex_acquire(&mutexMove, TM_INFINITE);
                        move->from_message(move, msg);
                        move->print(move);
                        rt_mutex_release(&mutexMove);
                        break;
                        
                    case MESSAGE_TYPE_MISSION:
                        rt_printf("tserver : Le message reçu %d est une mission\n",num_msg);
                        rt_mutex_acquire(&mutexMission, TM_INFINITE);
                        mission->from_message(mission, msg);
                        mission->print(mission);
                        rt_sem_v(&semMission);
                        rt_mutex_release(&mutexMission);
                        break;
                }
            }
            else
            {
                /* Gestion de la perte de communication avec le moniteur */
                perte_com_moniteur(STATUS_ERR_UNKNOWN);
            }
            rt_mutex_acquire(&mutexEtat,TM_INFINITE);
            status = etatCommMoniteur ;
            rt_mutex_release(&mutexEtat) ;
        }
    }
}

/**
 * Tâche périodeique envoyant régulièrement des messages aux robot
     quand ce dernier est connecté. 
 * PERIODE :  200s 
 * PRIORITE : 50
 */
void deplacer(void *arg) {
    int status = 2;
    int gauche;
    int droite;
    int bool_envoye ;
    int compteur ;
    
    rt_printf("tmove : Debut de l'éxecution de periodique à 1s\n");
    rt_task_set_periodic(NULL, TM_NOW, PERIODE_DEPLACER);
    
    while (1) {
        /* Attente de l'activation périodique */
        rt_task_wait_period(NULL);
        //rt_printf("tmove : Activation périodique\n");
        
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        status = etatCommRobot;
        rt_mutex_release(&mutexEtat);
        
        if (status == STATUS_OK) {
            rt_mutex_acquire(&mutexMove, TM_INFINITE);
            switch (move->get_direction(move)) {
                case DIRECTION_FORWARD:
                    gauche = MOTEUR_ARRIERE_LENT;
                    droite = MOTEUR_ARRIERE_LENT;
                    break;
                case DIRECTION_LEFT:
                    gauche = MOTEUR_ARRIERE_LENT;
                    droite = MOTEUR_AVANT_LENT;
                    break;
                case DIRECTION_RIGHT:
                    gauche = MOTEUR_AVANT_LENT;
                    droite = MOTEUR_ARRIERE_LENT;
                    break;
                case DIRECTION_STOP:
                    gauche = MOTEUR_STOP;
                    droite = MOTEUR_STOP;
                    break;
                case DIRECTION_STRAIGHT:
                    gauche = MOTEUR_AVANT_LENT;
                    droite = MOTEUR_AVANT_LENT;
                    break;
            }
            rt_mutex_release(&mutexMove);
            
            rt_mutex_acquire(&mutexCompteurCommRobot, TM_INFINITE) ;
            compteur = compteurEchecCommRobot ;
            rt_mutex_release(&mutexCompteurCommRobot) ;
            
            /* Tentatives succéssives d'envoyer un message au robot. 
             * set_motors
             */
            bool_envoye = 0 ;             
            while (compteur < MAX_TEST_COM && !bool_envoye){                
                status = robot->set_motors(robot, gauche, droite);
                rt_printf("tmove: status après set_motors:%d\n",status);                
                if (status != STATUS_OK){
                    rt_mutex_acquire(&mutexCompteurCommRobot, TM_INFINITE) ;
                    compteurEchecCommRobot++ ;
                    compteur = compteurEchecCommRobot ;
                    rt_mutex_release(&mutexCompteurCommRobot) ;
                }else{
                    rt_mutex_acquire(&mutexCompteurCommRobot, TM_INFINITE) ;
                    compteurEchecCommRobot = 0 ;
                    compteur = compteurEchecCommRobot ;
                    bool_envoye = 1 ;
                    rt_mutex_release(&mutexCompteurCommRobot) ;
                }
            }
            /* Gestion de la perte de communication avec le robot 
                (si le compteur dépasse la limite) */
            if (status != STATUS_OK) {
                perte_com_robot(status) ;
            }
        }
    }
}

int write_in_queue(RT_QUEUE *msgQueue, void * data, int size) {
    void *msg;
    int err;
    
    msg = rt_queue_alloc(msgQueue, size);
    memcpy(msg, &data, size);
    
    if ((err = rt_queue_send(msgQueue, msg, sizeof (DMessage), Q_NORMAL)) < 0) {
        rt_printf("Error msg queue send: %s\n", strerror(-err));
    }
    rt_queue_free(&queueMsgGUI, msg);
    
    return err;
}

/** 
 * Tâche gérant la batterie du robot :
 * Envoie un message au robot pour obtenir son niveau de batterie
 * Envoie ce dernier au moniteur pour l'afficher
 * 
 * PERIODE : 250ms
 * PRIORITE : 5 
 */
void gerer_batterie(void* arg) {
    int status ; 
    int vbat = -1;
    int ancienne_valeur = -1 ;
    int compteur ;
    int bool_envoye ;
    DMessage *message;
    
    // Variable globale de la batterie protégé par un mutex
    rt_mutex_acquire(&mutexBatterie, TM_INFINITE);
    batterie = d_new_battery();
    rt_mutex_release(&mutexBatterie) ;
    
    rt_task_set_periodic(NULL, TM_NOW, PERIODE_BATTERIE);
    rt_printf("tbatterie : Activation périodique\n");
    while (1) {
        /* Attente de l'activation périodique */
        rt_task_wait_period(NULL);
        rt_sem_p(&semBatterie, TM_INFINITE) ;
        
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        status = etatCommRobot;
        rt_mutex_release(&mutexEtat);
        
        if (status == STATUS_OK){
            
            /* Tentatives succéssives de communiquer avec le robot
             * Fonction : vbat()
             */
            bool_envoye = 0 ; 
            compteur = 0 ;
            while (compteur < MAX_TEST_COM && !bool_envoye){
                status = d_robot_get_vbat(robot,&vbat) ;
                rt_printf("tbatteire: status après get_vbat: %d\n",status) ;
                if (status != STATUS_OK){
                    rt_mutex_acquire(&mutexCompteurCommRobot, TM_INFINITE) ;
                    compteurEchecCommRobot++ ;
                    compteur = compteurEchecCommRobot ;
                    rt_mutex_release(&mutexCompteurCommRobot) ;
                }else{
                    rt_mutex_acquire(&mutexCompteurCommRobot, TM_INFINITE) ;
                    compteurEchecCommRobot = 0 ;
                    compteur = compteurEchecCommRobot ;
                    bool_envoye = 1 ;
                    rt_mutex_release(&mutexCompteurCommRobot) ;
                }
            }
            
            if (status != STATUS_OK) {
                perte_com_robot(status) ;
                rt_printf("tbatterie : perte comm robot status:%d\n",status) ;
            }else{
                rt_sem_v(&semBatterie) ;
                if (vbat < 0){
                    //rt_printf("tbatterie: Erreur récupération vbat.\n");
                }
                else
                {
                    
                    if (vbat != ancienne_valeur){
                        ancienne_valeur = vbat ;
                        
                        rt_mutex_acquire(&mutexBatterie, TM_INFINITE);
                        d_battery_set_level(batterie,vbat) ;
                        rt_mutex_release(&mutexBatterie) ;
                        
                        message = d_new_message();
                        d_message_put_battery_level(message,batterie);
                        if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
                            message->free(message);
                        }                    
                    }
                }
            }
            rt_mutex_release(&mutexCompteurCommRobot) ;            
        }
        rt_task_wait_period(NULL);
    }
}

/** 
 * Tâche de gestion du râfraichissement du watchdog
 * Cette dernière se lance quand le mutex est libéré lors de la connexion
 * 
 * PERIODE :  1s
 * PRIORITE : 90
 */
void recharger_watchdog(void *arg){
    int status ;
    int bool_envoye ;
    int compteur ;
    
    rt_task_set_periodic(NULL, TM_NOW, PERIODE_WATCHDOG);    
    rt_printf("twatchdog : Activation périodique\n");
    
    while(1){
        rt_sem_p(&semWatchDog,TM_INFINITE);
        rt_task_wait_period(NULL);
        
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        status = etatCommRobot;
        rt_mutex_release(&mutexEtat);
        
        /* Tentatives succéssives de communication avec le robot
         * Fonction : reload_wdt()
         */
        if(status== STATUS_OK){
            bool_envoye = 0 ; 
            rt_mutex_acquire(&mutexCompteurCommRobot, TM_INFINITE) ;
            while (compteur < MAX_TEST_COM && !bool_envoye){
                status = d_robot_reload_wdt(robot) ;
                rt_printf("twatchdog: status après rechargement : %d\n",status) ; 
                if (status != STATUS_OK){
                    compteurEchecCommRobot++ ;
                    compteur = compteurEchecCommRobot ;
                }else{
                    compteurEchecCommRobot = 0 ;
                    compteur = compteurEchecCommRobot ;
                    bool_envoye = 1 ;
                }
            }
            rt_mutex_release(&mutexCompteurCommRobot) ;
            
            /* Gestion de la perte de communication avec le robot */
            if (status != STATUS_OK) {
                perte_com_robot(status) ;
                rt_printf("twatchdog: perte comm robot status = %d\n",status);
            }else{            
                rt_printf("twatchdog: rafraichissement du watchdog\n");
                rt_sem_v(&semWatchDog) ;
            }
        }
        
    }
}

/** 
 * Tâche de la gestion de la webcam
 *   Acquisition période de l'image 
 *   Envoie de l'image au moniteur
 *   Calcul de la position
 * 
 * PERIODE :  600ms
 * PRIORITE : 10
 */
void gerer_image(void *arg){

    int status ;
    int action ;

    camera = d_new_camera() ;
    camera -> open(camera) ;

    DImage* image = d_new_image();
    DJpegimage* jpegimage = d_new_jpegimage();
    DMessage* message = d_new_message(); 
    DPosition* aux_position = d_new_position(); 

    rt_printf("timage: démarrage de la tâche (période 600ms)\n") ;
    rt_task_set_periodic(NULL, TM_NOW, PERIODE_IMAGE);
    rt_sem_v(&semImage);
    
    while (1){
        
        rt_task_wait_period(NULL);  //En attente d'activation 
        rt_sem_p(&semImage, TM_INFINITE) ;
        
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        status = etatCommMoniteur;
        rt_mutex_release(&mutexEtat);

        if (status == STATUS_OK){
            // Gestion de l'action
            
            rt_mutex_acquire(&mutexActionImage, TM_INFINITE) ;
            action = actionImage ;
            rt_mutex_release(&mutexActionImage) ; 
            
            switch(action){               
                /* Calcul de la position et envoie au moniteur de l'image */
                case ACTION_COMPUTE_CONTINUOUSLY_POSITION:
                    rt_printf("timage: compute continuously position\n");
                    
                    rt_mutex_acquire(&mutexImage,TM_INFINITE);
                    d_camera_get_frame(camera, image);
                    
                    rt_mutex_acquire(&mutexPosition,TM_INFINITE);
                    
                    if ((position = image->compute_robot_position(image, arena))==NULL)
                        position = image->compute_robot_position(image, NULL);
                    
                    d_imageshop_draw_position(image, position);
                    rt_mutex_release(&mutexPosition);
                    
                    d_jpegimage_compress(jpegimage, image) ;
                    message = d_new_message();
                    d_message_put_jpeg_image(message, jpegimage) ;

                    //Envoie de l'image avec la position tracée au moniteur
                    if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
                        message->free(message);
                    }
                    
                    rt_mutex_release(&mutexImage);

                    message = d_new_message() ;
                    d_message_put_position(message,aux_position);
                    if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
                        message->free(message);
                    }
                    
                    break;
                /* On retourne en mode normal après avoir fait l'acquisition
                   de la position. */
                case ACTION_STOP_COMPUTE_POSITION:  
                    rt_printf("tImage: stop compute position\n") ;
                    rt_mutex_acquire(&mutexActionImage, TM_INFINITE) ;
                    action = ACTION_STANDARD_ACQUISITION;   
                    rt_mutex_release(&mutexActionImage) ;
                    break;                
                 
                /* Mode d'acquisiton "normal" de l'image et envoie périodique */
                default:                   
                    rt_mutex_acquire(&mutexImage,TM_INFINITE);
                    d_camera_get_frame(camera, image) ;
                    d_jpegimage_compress(jpegimage, image) ;
                    message = d_new_message(); 
                    d_message_put_jpeg_image(message, jpegimage) ;

                    if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
                        message->free(message);
                    }
                    rt_mutex_release(&mutexImage);
                    break;
            }
        }
        rt_sem_v(&semImage) ;
    }

}

/**
 * Tâche non périodique gérant la demande de recherche d'arène
 * 
 * PRIORITE : 10 
 */
void calibrer(void *arg) {
    int action ;
    int priseSem= 0; 

    DImage* image = d_new_image();
    DJpegimage* jpegimage = d_new_jpegimage();
    DMessage* message = d_new_message(); 
    

    while(1) {
        rt_sem_p(&semCalibrer, TM_INFINITE);     //En attente du semaphore

        // Gestion de l'action
        rt_mutex_acquire(&mutexActionImage, TM_INFINITE) ;
        action = actionImage ;
        rt_mutex_release(&mutexActionImage) ; 

        switch(action) {            
            /* Recherche d'una arène et envoie du message au moniteur */
            case ACTION_FIND_ARENA:
                if (priseSem==0){
                    rt_sem_p(&semImage,TM_INFINITE);
                    priseSem = 1 ;
                }       
                
                rt_mutex_acquire(&mutexImage,TM_INFINITE);
                d_camera_get_frame(camera, image);
                
                
                rt_mutex_acquire(&mutexArena, TM_INFINITE);
                arena = d_image_compute_arena_position(image);
                rt_mutex_release(&mutexArena);
                
                d_imageshop_draw_arena(image,arena);
                d_jpegimage_compress(jpegimage,image);
                message = d_new_message(); 
                d_message_put_jpeg_image(message, jpegimage) ;
                
                //Envoie de l'image (avec l'arène dessinnée) au moniteur
                if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
                    message->free(message);
                }
                
                rt_mutex_release(&mutexImage);
                break;
            /* Arène validée et enregistrées. On retourne dans le mode normal. */
            case ACTION_ARENA_IS_FOUND:        
                rt_mutex_acquire(&mutexActionImage, TM_INFINITE) ;
                action = ACTION_STANDARD_ACQUISITION;   
                rt_mutex_release(&mutexActionImage) ;
                rt_sem_v(&semImage) ;
                priseSem = 0 ;
                break;
            /* Annulation de la recherche d'arène. On retourne en mode normal. */
            case ACTION_ARENA_FAILED:           
                arena = NULL;
                rt_mutex_acquire(&mutexActionImage, TM_INFINITE) ;
                action = ACTION_STANDARD_ACQUISITION;   
                rt_mutex_release(&mutexActionImage) ;
                rt_sem_v(&semImage) ;
                priseSem = 0 ;
                break;   
        }
    }
}

/**
 * Gestion de la perte de communication avec le robot
 * Lorsqu'une tâche repère la perte de communication après plusieurs tentatives,
 * on appelle cette fonction pour mettre l'information dans la variable gloabale. 
 * 
 * @param status
 */
void perte_com_robot(int status) {
    DMessage* message ; 
    
    rt_mutex_acquire(&mutexEtat, TM_INFINITE);
    etatCommRobot = status;
    rt_mutex_release(&mutexEtat);
    
    message = d_new_message();
    message->put_state(message, status);
    
    rt_printf("global : Envoi message perte com robot status =%d\n",status);
    if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
        message->free(message);
    }
}

/**
 * Gestion de la perte de communication avec le moniteur
 * Lorsque l'on détecte la perte de communication avec le moniteur,
 * cette fonction est appellé par la tâche qui a repéreé l'erreur pour 
 * propager l'informtion.
 * 
 * @param status
 */
void perte_com_moniteur(int status) { 
    //fermer la perte de connexion 
    rt_mutex_acquire(&mutexEtat, TM_INFINITE);
    etatCommMoniteur = status;
    rt_mutex_release(&mutexEtat);
    
    rt_mutex_acquire(&mutexServeur, TM_INFINITE);
    serveur->close(serveur);
    rt_mutex_release(&mutexServeur);
}

/**
 * Tâche apériodique de gestion des missions
 * 
 * PRIORITE : 10
 */
void executer_mission(void* arg){
    int status;
    DMission* mission_locale = d_new_mission();
    DMessage* message;
    DPosition* position_robot ;
    while(1){
        rt_printf("tmission : Attente d'une mission\n");
        rt_sem_p(&semMission, TM_INFINITE);
        
        /* Début de la mission*/
        rt_printf("tmission : Début de la mission\n");
        
        /* Copie locale de la variable globale mission globale*/
        rt_mutex_acquire(&mutexMission, TM_INFINITE);
        if(mission->type != MISSION_TYPE_STOP)
                (*mission_locale) = (*mission);
        rt_mutex_release(&mutexMission);
        
        /* Afficher les infos sur la mission*/
        d_mission_print(mission_locale);
        
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        status = etatCommRobot;
        rt_mutex_release(&mutexEtat);
        
        switch (mission_locale->type){
            case MISSION_TYPE_REACH_OBJECTIVE:
                break;
            case MISSION_TYPE_REACH_COORDINATE:
                                    
                    /* position actuelle du robot*/
                    rt_printf("tmission : Infos sur la position du robot\n");
                    position_robot = d_new_position();
                    *position_robot = *position ; 
                    
                if(status == STATUS_OK && position_robot !=NULL){
    
                    /* Distance à parcourir*/
                    int deltaX = (mission_locale->x - d_position_get_x(position_robot));
                    int deltaY = (mission_locale->y - d_position_get_y(position_robot));
                    int distance = sqrt(deltaX*deltaX + deltaY*deltaY);
                    
                    float orientation_robot = d_position_get_orientation(position_robot);
                    float angle_cible;
                        rt_printf("tmission positions : %d, %d, %d, %d\n", mission_locale->x,
                                mission_locale->y, d_position_get_x(position_robot),
                                d_position_get_y(position_robot));
                        rt_printf("tmission : orientation robot %f\n",orientation_robot);
                     if(deltaX >0 && deltaY>0 ){
                         angle_cible = atan2(deltaY,deltaX);
                     }
                     else if(deltaX <0 && deltaY>0 ){
                        angle_cible = PI-atan2(deltaY,-deltaX);
                     }
                     else if(deltaX >0 && deltaY<0 ){
                        angle_cible = 2*PI-atan2(-deltaY,deltaX);
                     }
                    else{
                          angle_cible = PI+atan2(-deltaY,-deltaX);
                    }
                    
                    float orientation = angle_cible - orientation_robot;
                    
                    /* Sens du robot*/
                    int sens = (orientation>0)? ANTI_HORAIRE : HORAIRE;
                    /* Angle en dégré*/
                    int angle = abs(orientation) * 180.0 / PI ; 

                    rt_printf("angle =%d, sens = %d, distance =%d  ", angle, sens, distance);
                    d_robot_turn(robot, angle, sens);

                    d_robot_move(robot, distance);
                    
                    /* attendre la fin de la mission*/
                    busy = 1;
                    while(busy != 0 ){
                        busy = d_robot_is_busy(robot, &busy);
                        rt_task_sleep(1);
                    }
                    
                }
                break;
            case MISSION_TYPE_STOP:
                /* Arrêter le robot*/
                rt_printf("tMission : Arrêt de la mission demandée");
                rt_mutex_acquire(&mutexEtat, TM_INFINITE);
                status = etatCommRobot;
                rt_mutex_release(&mutexEtat);
                if(status == STATUS_OK){
                    d_robot_stop(robot);
                }
                break;
        }
        /* Mission terminée !*/
        rt_printf("tMission : Mission terminée");
        message = d_new_message();
        d_message_mission_terminate(message, mission_locale->get_id);
        if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
            message->free(message);
        }
    }
}
